//
// GPSMapEdit
// (c) Konstantin Galichsky (kg@geopainting.com), 2002-2004
//
// NMEA-0183 protocol implementation.
//

# include "StdAfx.h"
# include "ComPort.h"
# include "NMEA0183.h"
# include "Positioning.h"
# include "ParseHelpers.h"
# include "Wrappers.h"
# include "Globals.h"

# ifdef _DEBUG
static char c_strTest [] = 
	"$GPRMC,181212,A,5548.4647,N,03730.3748,E,0.0,0.0,280403,9.5,E,A*11\r\n"
	"$GPRMB,A,,,,,,,,,,,,A,A*0B\r\n"
	"$GPGGA,181212,5548.4647,N,03730.3748,E,1,07,2.4,193.2,M,15.4,M,,*41\r\n"
	"$GPGLL,5548.4647,N,03730.3748,E,181212,A,A*4C\r\n"
	"$GPBOD,,T,,M,,*47\r\n"					//
	"$GPBWC,181212,,,,,,T,,M,,N,,A*72\r\n"	// Time
	"$GPVTG,0.0,T,350.5,M,0.0,N,0.0,K*4D\r\n" // Direction(true/magnetic),Speed
	"$GPXTE,A,A,,,N,A*51\r\n"
	"$PGRME,8.9,M,8.0,M,11.9,M*1E\r\n"	// Garmin: horiz/vert/overall precision
	"$PGRMZ,599,f*01\r\n"				// Garmin: Elevation
	"$PGRMM,WGS 84*06\r\n"				// Garmin: Datum
	"$HCHDG,78.9,,,9.5,E*13\r\n"        // magnetic compass (deg) & variation (East/West)
	"";
/*
$GPRMC,074504,A,5551.9159,N,03728.0879,E,0.0,0.0,290403,9.5,E,A*11
$GPRMB,A,,,,,,,,,,,,A,A*0B
$GPGGA,074504,5551.9159,N,03728.0879,E,1,04,4.9,149.9,M,15.4,M,,*44
$GPGLL,5551.9159,N,03728.0879,E,074504,A,A*4D
$GPBOD,,T,,M,,*47
$GPBWC,074504,,,,,,T,,M,,N,,A*79
$GPVTG,0.0,T,350.5,M,0.0,N,0.0,K*4D
$GPXTE,A,A,,,N,A*51
$PGRME,17.2,M,20.6,M,26.9,M*13
$PGRMZ,553,f*07
$PGRMM,WGS 84*06
$HCHDG,95.1,,,9.5,E*18
*/
# endif // _DEBUG

	// Find end of field.
static
const char * FindNextField (const char * _pBegin, const char * _pEnd) {
	for (const char * p = _pBegin; p < _pEnd; ++ p)
		if (* p == ',')
			return p + 1 < _pEnd ? p + 1 : _pEnd;

	return _pEnd;
}

static inline
size_t Get2Dec (const char * _str) {
	return (_str [0] - '0')*10 + (_str [1] - '0');
}

static
BYTE GetCheckSum (const char * _pStart, const char * _pEnd) {
	BYTE btRet = 0;
	for (const char * p = _pStart; p < _pEnd; ++ p)
		btRet ^= static_cast<BYTE> (* p);
	return btRet;
}

static
bool ParseNMEASentence (const char * _str, size_t _cLen, PositionInfo_t & _pi) {
# ifdef _DEBUG
/*	char buf [MAX_NMEA_SENTENCE + 1];
	assert (_cLen <= MAX_NMEA_SENTENCE);
	::memcpy (buf, _str, _cLen);
	buf [_cLen] = '\0';
	::OutputDebugString (buf);
	::OutputDebugString ("-----\n");*/
# endif // _DEBUG

	bool bNMEA = false;

	const char * pEnd = _str + _cLen;
	const char * pLine = _str;
	do {
		const char * const pNextLine = FindNextLine (pLine, pEnd);

		if (* pLine == '$' && pLine + 1 + 6 + 1 < pEnd) {
			bNMEA = true;

			// Check integrity by checksum.
			bool bCheckSumFailed = false;
			if (pNextLine [-5] == '*') {
				const BYTE btCheckSum = GetCheckSum (pLine + 1, pNextLine - 5);

				unsigned long ulCheckSumDef = 0;
				const char * p = NULL;
				ParseHex (pNextLine - 4, pNextLine - 2, & p, ulCheckSumDef);

				if (btCheckSum != ulCheckSumDef)
					bCheckSumFailed = true;
			}

			if (bCheckSumFailed) {
				// TODO: ??
			} else if (::memcmp (pLine + 1, "GPRMC,", 6) == 0) {
				//
				// The Recommended Mimimum.
				//

				const char * pTime = pLine + 7;
				const char * pA    = FindNextField (pTime, pEnd);
				if (* pA == 'A') {
					const char * pLat  = FindNextField (pA,    pEnd);
					const char * pNS   = FindNextField (pLat,  pEnd);
					const char * pLon  = FindNextField (pNS,   pEnd);
					const char * pEW   = FindNextField (pLon,  pEnd);

					if (pLat < pNS - 1 && pLon < pEW - 1) {
						const long ulLatDeg = ParseDecU (pLat, pLat + 2);
						const float fLatMin = ParseReal (pLat + 2, pNS - 1);
						const float fLat = (* pNS == 'S' ? -1 : 1)*(ulLatDeg + fLatMin/60); 

						const long ulLonDeg = ParseDecU (pLon, pLon + 3);
						const float fLonMin = ParseReal (pLon + 3, pEW - 1);
						const float fLon = (* pEW == 'W' ? -1 : 1)*(ulLonDeg + fLonMin/60); 
						
						_pi.SetXY (fLon, fLat);
					}

					const char * pSpeedInKnots        = FindNextField (pEW,                pEnd);
					const char * pAzimuth             = FindNextField (pSpeedInKnots,      pEnd);
					const char * pDate                = FindNextField (pAzimuth,           pEnd);
					const char * pMagneticVariation   = FindNextField (pDate,              pEnd);
					const char * pMagneticVariationEW = FindNextField (pMagneticVariation, pEnd);

					if (pSpeedInKnots < pAzimuth - 1) {
						const float fSpeedInKnots  = ParseReal (pSpeedInKnots, pAzimuth - 1);
						_pi.SetSpeed (fSpeedInKnots*1.852f);
					}

					if (pAzimuth < pDate - 1) {
						const float fAzimuth = ParseReal (pAzimuth, pDate - 1);
						_pi.SetAzimuth (fAzimuth);
					}

					if (pMagneticVariation < pMagneticVariationEW - 1) {
						const float fMagneticVariation = (* pMagneticVariationEW == 'E' ? 1 : -1)*ParseReal (pMagneticVariation, pMagneticVariationEW - 1);
						_pi.SetMagneticVariation (fMagneticVariation);
					}

					if (pDate <= pMagneticVariation - 7 && pTime <= pA - 7) {
						SYSTEMTIME st;
						st.wYear   = Get2Dec (pDate + 4) + 2000;
						st.wMonth  = Get2Dec (pDate + 2);
						st.wDay    = Get2Dec (pDate);
						st.wDayOfWeek = 0;
						st.wHour   = Get2Dec (pTime);
						st.wMinute = Get2Dec (pTime + 2);
						st.wSecond = Get2Dec (pTime + 4);
						st.wMilliseconds = 0;

						DATE dt;
						::SystemTimeToVariantTime (& st, & dt);
						// NOTE: SystemTimeToVariantTime() ignores milliseconds
						if (pTime [6] == '.')
							dt += ParseReal (pTime + 6, pA - 1)/(60*60*24);

						_pi.SetTime (dt);
					}
				}
			} else if (::memcmp (pLine + 1, "GPGGA,", 6) == 0) {
				//
				// The current fix data.
				//

				const char * const pTime         = pLine + 7;
				const char * const pLat          = FindNextField (pTime, pEnd);
				const char * const pNS           = FindNextField (pLat,  pEnd);
				const char * const pLon          = FindNextField (pNS,   pEnd);
				const char * const pEW           = FindNextField (pLon,  pEnd);
				const char * const pFixQuality   = FindNextField (pEW,           pEnd);
				const char * const pSatellites   = FindNextField (pFixQuality,   pEnd);
				const char * const pDilution     = FindNextField (pSatellites,   pEnd);
				const char * const pAltitude     = FindNextField (pDilution,     pEnd);
				const char * const pAltitudeUnit = FindNextField (pAltitude,     pEnd);
				const char * const pGeoidHeight  = FindNextField (pAltitudeUnit, pEnd);
				const char * const pGeoidHUnit   = FindNextField (pGeoidHeight,  pEnd);

				if (pLat < pNS - 1 && pLon < pEW - 1) {
					const long ulLatDeg = ParseDecU (pLat, pLat + 2);
					const float fLatMin = ParseReal (pLat + 2, pNS - 1);
					const float fLat = (* pNS == 'S' ? -1 : 1)*(ulLatDeg + fLatMin/60); 

					const long ulLonDeg = ParseDecU (pLon, pLon + 3);
					const float fLonMin = ParseReal (pLon + 3, pEW - 1);
					const float fLon = (* pEW == 'W' ? -1 : 1)*(ulLonDeg + fLonMin/60); 
					
					_pi.SetXY (fLon, fLat);
				}

				if (pSatellites < pDilution - 1) {
					const size_t cSatellites = ParseDecU (pSatellites, pDilution - 1);
					_pi.SetSatellites (cSatellites);
				}

				if (pDilution < pAltitude - 1) {
					const float fDilution = ParseReal (pDilution, pAltitude - 1);
//					_pi.SetXYPrec (fDilution*c_fOneDOP);
				}

				if (pFixQuality < pSatellites - 1) {
					const size_t cFix = pFixQuality [0] - '0';
					switch (cFix) {
						case 0:
							_pi.SetFix (PositionInfo_t::fixInvalid);
							_pi.dwDef &= ~PositionInfo_t::defXY;
							_pi.dwDef &= ~PositionInfo_t::defXYPrec;
							_pi.dwDef &= ~PositionInfo_t::defAltitudePrec;
						    break;

						case 1:  _pi.SetFix (PositionInfo_t::fix3D);         break;
						case 2:  _pi.SetFix (PositionInfo_t::fix3D_diff);    break;
						case 3:	 _pi.SetFix (PositionInfo_t::fixPrec);       break;
						case 4:  _pi.SetFix (PositionInfo_t::fixRTK);        break;
						case 5:  _pi.SetFix (PositionInfo_t::fixFRTK);       break;
						case 6:  _pi.SetFix (PositionInfo_t::fixEstimated);  break;
						case 7:  _pi.SetFix (PositionInfo_t::fixManual);     break;
						case 8:  _pi.SetFix (PositionInfo_t::fixSimulation); break;
						default: _pi.SetFix (PositionInfo_t::fixUnknown);    break;
					}
				}

				if (pAltitude < pAltitudeUnit - 1) {
					const float fAltitude = (* pAltitudeUnit == 'M' ? 1. : 1./c_feet_per_m)*ParseReal (pAltitude, pAltitudeUnit - 1);
					_pi.SetAltitude (fAltitude);
				}

				if (pGeoidHeight < pGeoidHUnit - 1) {
					const float fGeoidHeight = (* pGeoidHUnit == 'M' ? 1. : 1./c_feet_per_m)*ParseReal (pGeoidHeight, pGeoidHUnit - 1);
					_pi.SetGeoidHeight (fGeoidHeight);
				}
			} else if (::memcmp (pLine + 1, "GPGLL,", 6) == 0) {
				//
				// Geographic Latitude and Longitude.
				//

				const char * const pLat  = pLine + 7;
				const char * const pNS   = FindNextField (pLat,  pEnd);
				const char * const pLon  = FindNextField (pNS,   pEnd);
				const char * const pEW   = FindNextField (pLon,  pEnd);
				const char * const pTime = FindNextField (pEW,   pEnd);
				const char * const pA    = FindNextField (pTime, pEnd);

				if (* pA == 'A' && pLat < pNS - 1 && pLon < pEW - 1) {
					const long ulLatDeg = ParseDecU (pLat, pLat + 2);
					const float fLatMin = ParseReal (pLat + 2, pNS - 1);
					const float fLat = (* pNS == 'S' ? -1 : 1)*(ulLatDeg + fLatMin/60); 

					const long ulLonDeg = ParseDecU (pLon, pLon + 3);
					const float fLonMin = ParseReal (pLon + 3, pEW - 1);
					const float fLon = (* pEW == 'W' ? -1 : 1)*(ulLonDeg + fLonMin/60); 
					
					_pi.SetXY (fLon, fLat);
				}
			} else if (::memcmp (pLine + 1, "GPVTG,", 6) == 0) {
				//
				// Velocity made good.
				//

				// TODO:
			} else if (::memcmp (pLine + 1, "HCHDG,", 6) == 0) {
				//
				// Magnetic compass info.
				//
				const char * pAzimuth = pLine + 7;
				const char * p2       = FindNextField (pAzimuth, pEnd);

				if (pAzimuth < p2 - 1) {
					const float fAzimuth = ParseReal (pAzimuth, p2 - 1);
					_pi.SetAzimuth (fAzimuth);
				}
			} else if (::memcmp (pLine + 1, "PGRMZ,", 6) == 0) {
				//
				// Garmin proprietary: altitude info.
				//
				const char * pAltitude     = pLine + 7;
				const char * pAltitudeUnit = FindNextField (pAltitude, pEnd);

				if (pAltitude < pAltitudeUnit - 1) {
					const float fAltitude = (* pAltitudeUnit == 'f' ? 1/c_feet_per_m : 1)*ParseReal (pAltitude, pAltitudeUnit - 1);
					_pi.SetAltitude (fAltitude);
				}
			} else if (::memcmp (pLine + 1, "PGRME,", 6) == 0) {
				//
				// Garmin proprietary: estimated error.
				//
				const char * pXYPrec     = pLine + 7;
				const char * pXYPrecUnit = FindNextField (pXYPrec, pEnd);

				if (pXYPrec < pXYPrecUnit - 1) {
					const float fXYPrec = (* pXYPrecUnit == 'M' ? 1 : 1/c_feet_per_m)*ParseReal (pXYPrec, pXYPrecUnit - 1);
					_pi.SetXYPrec (fXYPrec);
				}

				const char * pAltitudePrec     = FindNextField (pXYPrecUnit,   pEnd);
				const char * pAltitudePrecUnit = FindNextField (pAltitudePrec, pEnd);

				if (pAltitudePrec < pAltitudePrecUnit - 1) {
					const float fAltitudePrec = (* pAltitudePrecUnit == 'M' ? 1 : 1/c_feet_per_m)*ParseReal (pAltitudePrec, pAltitudePrecUnit - 1);
					_pi.SetAltitudePrec (fAltitudePrec);
				}
			}
		}

		pLine = pNextLine;
	} while (pLine < pEnd);

	return bNMEA;
}

///////////////////////////////////

void CNMEA0183::GetName (string_t & _strName) const {
	_strName.append ("NMEA-0183");
}

void CNMEA0183::Start () {
# ifdef _DEBUG
//	ParseNMEASentence (c_strTest, sizeof (c_strTest), m_pi);
# endif // _DEBUG

	// Setup COM port.
	assert (m_pPort);
	m_pPort->SetDCB (CBR_4800, 8, false, ONESTOPBIT);

	COMMTIMEOUTS ct = {100, 10, 10, 10, 10};
	::SetCommTimeouts (* m_pPort, & ct);

	m_pPort->ClearQueues ();

	// Request GPGGA sentence.
//	const char strRequestGPGGA [] = "$CCGPQ,GGA\r\n";
//	DWORD dwWritten = 0;
//	::WriteFile (* m_pPort, strRequestGPGGA, sizeof (strRequestGPGGA), & dwWritten, NULL);

	// Initialize data receiving.
	m_bDirty = false;
	m_cSentenceLen = 0;
	m_dwLastDataTime = ::GetTickCount ();
}

CGpsProtocol::Status_t CNMEA0183::OnDataReceived (const BYTE * _pData, size_t _cSize) {
	const DWORD dwElapsed = ::GetTickCount () - m_dwLastDataTime;
	if (_cSize == 0) {
		// Detect long silence.
		if (dwElapsed > 5000)
			return stSureError;

		// Detect short time-out between bunch of sentencies.
		if (m_bDirty && dwElapsed > 100) {
			PushPositionInfo (m_pi);
			m_pi = PositionInfo_t ();
			m_bDirty = false;
			m_dwLastSaveTime = ::GetTickCount ();
			return stSureOK;
		}

		return stNotSure;
	}
	m_dwLastDataTime = ::GetTickCount ();

	if (m_bDirty && ::GetTickCount () - m_dwLastSaveTime > 3000) {
		PushPositionInfo (m_pi);
		m_pi = PositionInfo_t ();
		m_bDirty = false;
		m_dwLastSaveTime = ::GetTickCount ();
	}


	//
	// Compile data pieces into sentencies.
	//

	const char * pStart = reinterpret_cast<const char *> (_pData);
	const char * pEnd = pStart + _cSize;

	const char * pNext = reinterpret_cast<const char *> (::memchr (pStart, '$', _cSize));
	if (pNext == NULL) {
		// Append to the end of buffer.
		if (m_cSentenceLen + _cSize < sizeof (m_strSentence)) {
			::memcpy (m_strSentence + m_cSentenceLen, _pData, _cSize);
			m_cSentenceLen += _cSize;
			return m_bDirty ? stSureOK : stNotSure;
		} else
			// Buffer overflow. NOTE: NMEA requires any sentence to fit the buffer.
			return stSureError;
	} else {
		const size_t cTailLen = pNext - pStart;
		if (m_cSentenceLen + cTailLen >= sizeof (m_strSentence))
			// Buffer overflow.
			return stSureError;

		// Append to the end of buffer.
		::memcpy (m_strSentence + m_cSentenceLen, pStart, cTailLen);
		m_cSentenceLen += cTailLen;

		if (ParseNMEASentence (m_strSentence, m_cSentenceLen, m_pi))
			m_bDirty = true;
	}

	while (true) {
		assert (pNext && pNext < pEnd && * pNext == '$');
		pStart = pNext;
		pNext = reinterpret_cast<const char *> (::memchr (pStart + 1, '$', pEnd - pStart - 1));
		if (pNext == NULL) {
			// Save to the buffer.
			m_cSentenceLen = pEnd - pStart;
			::memcpy (m_strSentence, pStart, m_cSentenceLen);
			return m_bDirty ? stSureOK : stNotSure;
		}

		if (ParseNMEASentence (pStart, pNext - pStart, m_pi))
			m_bDirty = true;
		else
			return stSureError;
	}
}
